﻿using System.Collections;
using System.Collections.Generic;
using UnityEngine;

namespace NinjaToolbox.PhysicsSystem
{
    public static class PhysicsUtil
    {
        public struct StepMoveOptions
        {
            public float StepLength;
            public float Error;
            public int MaxIterateCount;


            public static StepMoveOptions Create(float stepLength = 0.3f)
            {
                return new StepMoveOptions()
                {
                    StepLength = stepLength,
                    Error = 0.05f,
                    MaxIterateCount = 600,
                };
            }
        }

        public static readonly float Infinity = 100000.0f;


        public static bool StepMoveTo(PhysicsObject physicsObject, Vector3 to, StepMoveOptions options)
        {
            if (physicsObject.ColliderShape.IsHit(to)) return false;

            var edgePoint = Vector3.zero;

            edgePoint = physicsObject.ColliderShape.MassPointToEdge(physicsObject.transform.position, to);

            var moveValue = to - edgePoint;
            var firstIntersect = default(OBBBox);

            return StepMove(physicsObject, moveValue, out firstIntersect, options);
        }

        public static bool StepMove(PhysicsObject physicsObject, Vector3 moveValue, StepMoveOptions options)
        {
            var firstIntersect = default(OBBBox);

            return StepMove(physicsObject, moveValue, out firstIntersect, options);
        }

        public static bool StepMove(PhysicsObject physicsObject, Vector3 moveValue, out OBBBox firstIntersect, StepMoveOptions options)
        {
            var result = true;
            firstIntersect = default(OBBBox);

            var direction = moveValue.normalized;
            var stepLength = options.StepLength;
            var step = direction * stepLength;

            if (step.magnitude > moveValue.magnitude)
                step = moveValue;

            var to = physicsObject.transform.position + moveValue;
            var errorCounter = 0;

            var canNext = true;
            while (canNext)
            {
                errorCounter++;
                if (errorCounter > options.MaxIterateCount)
                {
                    Debug.LogError("StepMove While Crashed!");
                    result = false;
                    break;
                }
                //如果超出最大迭代数量就报错

                var isIntersectWithSceneColliders = false;
                var cachePosition = physicsObject.transform.position;
                physicsObject.transform.position += step;
                isIntersectWithSceneColliders = PhysicsSystemManager.Instance.IsIntersectWithSceneColliders(physicsObject.ColliderShape, out firstIntersect);
                physicsObject.transform.position = cachePosition;

                if (isIntersectWithSceneColliders)
                {
                    canNext = false;
                    result = false;
                }
                else
                {
                    physicsObject.transform.position += step;//执行一次移动
                }

                if (canNext)//距离跳出判断
                    canNext = Vector3.Distance(physicsObject.transform.position, to) > options.Error;

                if (canNext)//方向跳出判断
                {
                    var currentDirection = (to - physicsObject.transform.position).normalized;
                    var dot = Vector3.Dot(direction, currentDirection);
                    if (dot < 0.1f)
                        canNext = false;
                }
            }

            return result;
        }
    }
}
